#include <imp/imp_log.h>
#include <imp/imp_common.h>
#include <system/system.h>
#include <ivs_base_move.h>
#include <ivs_base_move_sad.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#define VERSION_SKIP_FRAME 1
#define TAG  "IVS_MOVE"

typedef struct {
	int width;
	int height;
	uint8_t** bufs;

	int ref_Num;
	int skipFrameCnt;
	int frame_id;
	int do_id;
	int NumOfHistory;
	int id_new;
	int id_old;
	int start_fid;
	int start_did;

	int thresh;
	int SADMode;
	int retForSad;
	uint8_t* frame_data;
	int isUseSimd;

	IMat buf_idx1;
	IMat buf_idx2;
	IMat buf_temp;
	IMat dst;
} IVSMove_c;


static IVSMove_c* IVSMove_init(int _width,int _height,int _skipFrameCnt, int _referenceNum, int _sadmode, int sense)
{
	int i = 0;
	if((_skipFrameCnt<0)||(_referenceNum<=0))
	{
		IMP_LOG_ERR(TAG, "skipFrameCnt or referenceNum out of range\n");
		goto error_param_set;
	}
	IVSMove_c* ivsMove = (IVSMove_c*)calloc(1, sizeof(IVSMove_c));
	if (NULL == ivsMove) {
		IMP_LOG_ERR(TAG, "malloc ivsMove failed\n");
		goto err_malloc_ivsMove;
	}

	ivsMove->width = _width;
	ivsMove->height = _height;

	ivsMove->ref_Num = _referenceNum;
	ivsMove->skipFrameCnt = _skipFrameCnt+1;
	ivsMove->frame_id = 0;
	ivsMove->do_id = 0;
	ivsMove->NumOfHistory = 0;
	ivsMove->id_new = 0;
	ivsMove->id_old = 0;
	ivsMove->start_fid = 0;
	ivsMove->start_did = 0;

	if(ivsMove->ref_Num <= ivsMove->skipFrameCnt)
		ivsMove->NumOfHistory = 2;
	else
		ivsMove->NumOfHistory = (ivsMove->ref_Num/ivsMove->skipFrameCnt) + ((ivsMove->ref_Num%ivsMove->skipFrameCnt)>0) + 1;

	ivsMove->start_fid = ivsMove->do_id*ivsMove->skipFrameCnt - ivsMove->ref_Num;
	while (ivsMove->start_fid < 0) {
		ivsMove->do_id++;
		ivsMove->start_fid = ivsMove->do_id*ivsMove->skipFrameCnt - ivsMove->ref_Num;
	}
	ivsMove->start_did = ivsMove->do_id*ivsMove->skipFrameCnt;

	ivsMove->SADMode = _sadmode;
	ivsMove->retForSad = 0;
	if(ivsMove->SADMode != 0){
		IMP_LOG_ERR(TAG, "sad mode error\n");
		goto err_sad_mode;
	}

	ivsMove->isUseSimd = is_has_simd128();
	if (!!ivsMove->isUseSimd) {
		mk();
	}

	//ivsMove->isUseSimd = is_has_simd128();
	ivsMove->bufs = (uint8_t**)calloc(ivsMove->NumOfHistory, sizeof(uint8_t*));
	if (NULL == ivsMove->bufs) {
		IMP_LOG_ERR(TAG, "malloc bufs error\n");
		goto err_malloc_bufs;
	}

	for(i = 0; i < ivsMove->NumOfHistory; i++){
		ivsMove->bufs[i] = (uint8_t*)calloc(ivsMove->width*ivsMove->height, sizeof(uint8_t));
		if (NULL == ivsMove->bufs[i]) {
			IMP_LOG_ERR(TAG, "malloc bufs[%d] error\n", i);
			goto err_malloc_buf;
		}
	}

	ivsMove->frame_data = (uint8_t*)calloc(ivsMove->width*ivsMove->height, sizeof(uint8_t));
	if (NULL==ivsMove->frame_data){
	  IMP_LOG_ERR(TAG, "calloc frame_data failed!\n");
	  goto err_calloc_frame_data;
	}

	ivsMove->buf_idx1.cols = ivsMove->width;
	ivsMove->buf_idx1.rows = ivsMove->height;
	ivsMove->buf_idx1.step = ivsMove->width*sizeof(uint8_t);
	ivsMove->buf_idx2.cols = ivsMove->width;
	ivsMove->buf_idx2.rows = ivsMove->height;
	ivsMove->buf_idx2.step = ivsMove->width*sizeof(uint8_t);
	ivsMove->buf_temp.cols = ivsMove->width;
	ivsMove->buf_temp.rows = ivsMove->height;
	ivsMove->buf_temp.step = ivsMove->width*sizeof(uint8_t);

	switch(ivsMove->SADMode)
	{
	case 0://8*8
		ivsMove->dst.cols = ivsMove->width>>3;
		ivsMove->dst.rows = ivsMove->height>>3;
		ivsMove->dst.step = (ivsMove->width>>3)*sizeof(uint8_t);
		break;
	default:
		break;
	}

	ivsMove->thresh = 0;
	switch(sense)
	{
	case 0:
		ivsMove->thresh = 30;
		break;
	case 1:
		ivsMove->thresh = 20;
		break;
	case 2:
		ivsMove->thresh = 15;
		break;
	case 3:
		ivsMove->thresh = 10;
		break;
	default:
		break;
	}
	return ivsMove;

err_calloc_frame_data:
err_malloc_buf:
	i = ivsMove->NumOfHistory;
	for(--i; i >= 0; i--){
		if(ivsMove->bufs[i]){
			free(ivsMove->bufs[i]);
			ivsMove->bufs[i] = NULL;
		}
	}
	free(ivsMove->bufs);
	ivsMove->bufs = NULL;
err_malloc_bufs:
err_sad_mode:
	free(ivsMove);
	ivsMove = NULL;
err_malloc_ivsMove:
	return NULL;
error_param_set:
	return NULL;
}

static int reset_param(IVSMove_c* ivsMove, int _sadmode, int sense)
{
	ivsMove->SADMode = _sadmode;
	if (sense < 0 || sense > 3){
		IMP_LOG_ERR(TAG, "sense value error, %d\n", sense);
		return -1;
	}
	switch (sense) {
	case 0:
		ivsMove->thresh = 30;
		break;
	case 1:
		ivsMove->thresh = 20;
		break;
	case 2:
		ivsMove->thresh = 15;
		break;
	case 3:
		ivsMove->thresh = 10;
		break;
	default:
		break;
	}
	if (ivsMove->SADMode != 0) {
		IMP_LOG_ERR(TAG, "sad mode error %d\n", _sadmode);
		return -1;
	}
	switch (ivsMove->SADMode) {
	case 0://8*8
		ivsMove->dst.cols = ivsMove->width>>3;
		ivsMove->dst.rows = ivsMove->height>>3;
		ivsMove->dst.step = (ivsMove->width>>3)*sizeof(uint8_t);
		break;
	default:
		break;
	}
	return 0;
}


//#define TIME
static int update_mhi( IVSMove_c* ivsMove, ivs_move_t* mv, uint8_t* data, IMP_IVS_BaseMoveOutput* res)
{
#ifdef TIME
	struct timeval tv;
	gettimeofday(&tv, NULL);
	uint64_t time_last = tv.tv_sec*1000000 + tv.tv_usec;
#endif
	bool ref_cond, cur_cond;
	if(ivsMove->frame_id < ivsMove->start_fid)
	{
		ivsMove->frame_id++;
		res->ret = 0;
	}else
	{
		ivsMove->retForSad = 0;
		cur_cond = (ivsMove->frame_id % ivsMove->skipFrameCnt)==0;
		ref_cond = ((ivsMove->frame_id + ivsMove->ref_Num)%ivsMove->skipFrameCnt)==0;
		if((cur_cond)&&(ivsMove->frame_id >= ivsMove->start_did)){
			/*memcpy*/
			memcpy(ivsMove->bufs[ivsMove->id_new], data, ivsMove->width*ivsMove->height*sizeof(uint8_t));
			/*do*/
			uint8_t* data1 = ivsMove->bufs[ivsMove->id_new];
			uint8_t* data2 = ivsMove->bufs[ivsMove->id_old];
			ivsMove->buf_idx1.data = data1;
			ivsMove->buf_idx2.data = data2;
			ivsMove->buf_temp.data = data;
			ivsMove->dst.data = res->data;
			//printf("save %d and do[%d, %d] [%p, %p]!!!!\n",ivsMove->id_new, ivsMove->id_old, ivsMove->id_new, data1, data2);
			if (0 == sad(ivsMove->buf_idx1, ivsMove->buf_idx2, ivsMove->buf_temp, ivsMove->dst, ivsMove->SADMode, ivsMove->thresh, ivsMove->isUseSimd))
				ivsMove->retForSad = 1;

			if(ref_cond){
				/*update*/
				ivsMove->id_new++;
				ivsMove->id_new = ivsMove->id_new%ivsMove->NumOfHistory;
				if(ivsMove->id_new == ivsMove->id_old)
				{
					ivsMove->id_old++;
					ivsMove->id_old = ivsMove->id_old%ivsMove->NumOfHistory;
				}
			}
			ivsMove->do_id++;
		}
		else if(ref_cond){
			//printf("save %d!!!\n", ivsMove->id_new);
			/*memcpy*/
			memcpy(ivsMove->bufs[ivsMove->id_new], data, ivsMove->width*ivsMove->height*sizeof(uint8_t));
			/*update*/
			ivsMove->id_new++;
			ivsMove->id_new = ivsMove->id_new%ivsMove->NumOfHistory;
			if(ivsMove->id_new==ivsMove->id_old)
			{
				ivsMove->id_old++;
				ivsMove->id_old = ivsMove->id_old%ivsMove->NumOfHistory;
			}

		}//else if(ref_cond)
		ivsMove->frame_id++;
	}

	res->ret = ivsMove->retForSad;
	res->datalen = ivsMove->dst.cols*ivsMove->dst.rows*sizeof(uint8_t);
#ifdef TIME
	gettimeofday(&tv, NULL);
	time_last = tv.tv_sec*1000000 + tv.tv_usec - time_last;
	uint64_t time_ms = time_last*1.0/1000;
	printf("last time : %llu us\n", time_last);
	IMP_LOG_ERR(TAG, "last time : %llu us\n", time_last);
#endif
	return 0;
}


ivs_move_t* imp_alloc_base_move(IMP_IVS_BaseMoveParam* param, int (*free_data)(void *data))
{
	ivs_move_t *move = (ivs_move_t *)calloc(1, sizeof(ivs_move_t));
	if (move == NULL) {
		IMP_LOG_ERR(TAG, "calloc ivs move failed\n");
		goto err_calloc_move;
	}

	memcpy(&move->param, param, sizeof(IMP_IVS_BaseMoveParam));

	move->index = 0;
	move->free_data = free_data;

	move->ivs_move = IVSMove_init(move->param.frameInfo.width,
								  move->param.frameInfo.height,
								  move->param.skipFrameCnt,
								  move->param.referenceNum,
								  move->param.sadMode,
								  move->param.sense);
	if(move->ivs_move==NULL) {
		IMP_LOG_ERR(TAG, "IVSMove_init failed\n");
		goto err_IVSMove_init;
	}

	return move;

err_IVSMove_init:
	free(move);
err_calloc_move:
	return NULL;
}

void imp_free_base_move(ivs_move_t *move)
{
	if(!move)
	  return;

	IVSMove_c* ivs_mv = ((IVSMove_c*)(move->ivs_move));

	if (ivs_mv->frame_data != NULL) {
		free(ivs_mv->frame_data);
		ivs_mv->frame_data = NULL;
	}

	int i;
	for (i = 0; i < ivs_mv->NumOfHistory; i++) {
		if(ivs_mv->bufs[i]){
			free(ivs_mv->bufs[i]);
			ivs_mv->bufs[i] = NULL;
		}
	}

	if(ivs_mv->bufs){
		free(ivs_mv->bufs);
		ivs_mv->bufs = NULL;
	}

	free(ivs_mv);
	ivs_mv = NULL;

	free(move);
	move = NULL;
}

int imp_base_move_preprocess(ivs_move_t *move, IMPFrameInfo *frame)
{
  int size_of_frame = frame->width*frame->height;
  memcpy(((IVSMove_c*)(move->ivs_move))->frame_data, (void *)frame->virAddr, size_of_frame*sizeof(uint8_t));
  return 1;
}

int imp_base_move_process(ivs_move_t *move, IMPFrameInfo *frame, IMP_IVS_BaseMoveOutput *result)
{
	int ret = 0;

	if(move == NULL) {
		IMP_LOG_ERR(TAG, "move is NULL !\n");
		goto err_move_is_null;
	}

	if (move->free_data){
		move->free_data((void *)(frame->virAddr));
	}

	if(result == NULL) {
		IMP_LOG_ERR(TAG, "result is NULL !\n");
		goto err_result_is_null;
	}

	if (!!(((IVSMove_c*)(move->ivs_move))->isUseSimd)) {
		/* Insert MXU key first. */
		mk();
	}
	{
		char access_name[50];
		sprintf(access_name, "/tmp/mountdir/ivsbasemovesnap.nv12");
		if (access(access_name, R_OK | W_OK) == 0) {
			struct stat stat_buf;
			stat(access_name, &stat_buf);
			int snapsize = frame->width * frame->height * 3 / 2;
			if (stat_buf.st_size < snapsize*2) {
				int ispsnapfd = open(access_name, O_RDWR | O_APPEND);
				if (ispsnapfd >= 0) {
					write(ispsnapfd, (void *)frame->virAddr, frame->width * frame->height);
					write(ispsnapfd, (void *)frame->virAddr + frame->width * ((frame->height + 15) & ~15),frame->width * frame->height / 2);
					close(ispsnapfd);
				}
			}
		}
	}
	ret = update_mhi((IVSMove_c*)move->ivs_move, move, ((IVSMove_c*)(move->ivs_move))->frame_data, result);
	result->timeStamp = frame->timeStamp;
	return ret;

err_result_is_null:
err_move_is_null:
	return -1;
}

int imp_get_base_move_param(ivs_move_t *move, IMP_IVS_BaseMoveParam *param)
{
	if (!move) {
		IMP_LOG_ERR(TAG, "move is NULL\n");
		return -1;
	}
	memcpy(param, &move->param, sizeof(IMP_IVS_BaseMoveParam));

	return 0;
}

int imp_set_base_move_param(ivs_move_t *move, IMP_IVS_BaseMoveParam *param)
{
	if (!move || !param) {
		IMP_LOG_ERR(TAG, "move=%p, param=%p is NULL\n");
		return -1;
	}
	IVSMove_c* ivs_mv = ((IVSMove_c*)(move->ivs_move));

	return reset_param(ivs_mv, param->sadMode, param->sense);
}

int imp_base_flush_frame(ivs_move_t *move)
{
	return 0;
}
